#include "fir.h"
#include "math.h"
#include <stdlib.h>
#include "fft.h"
#define MATH_PI 3.1415926535
void fir_get_zp(float freq, STRZPNodeType *zps, U16 zpssize, STRZPNodeType *calz, U16 *RN) {
    float fN, fs;
    U32 N;
    U16 i,j,k;
    float dfs, nf;
    float hwfi, hwti, hwf, hwt, haf, hat, pk, pa, hwn, han;
    fs = freq * 128;
    fN = fs / (freq / 4);
    dfs = freq /4;
    N = (int)fN;
    for(j = 0; j < N; j++) {
        nf = dfs * j;
        hwfi = 0;
        hwti = 100;
        hwf = 1;
        hwt = 1;
        haf = 0;
        hat = 0;
				for(k = 0; k < (zpssize - 1); k++){
					if((nf >= zps[k].freq) && (nf < zps[k+1].freq)){
						
						hwfi = zps[k].freq;
						hwf = zps[k].amp;
						haf = zps[k].angle;

						hwti = zps[k + 1].freq;
						hwt = zps[k + 1].amp;
						hat = zps[k + 1].angle;

						break;
					}
				}
        pk = ((hwt - hwf) / (hwti - hwfi));
        hwn = pk * nf + hwf - pk * hwfi;


        if(hat < haf){
            hat = hat + 2 * MATH_PI;
				}

        pa = ((hat - haf) / (hwti - hwfi));
        han = pa * nf+ haf - pa * hwfi;
				calz[j].amp = nf;
				calz[j].amp = hwn;
				calz[j].angle = han;
    }
		*RN = N;
}

void fir_get_coef(STRZPNodeType *zps, U16 N, float df, U8 linera, float *coef){
	U16 M, k;
	float phase;
	float f[1024][2];
	M = N + 1;
	for(k = 0; k <= N/2; k++){
		phase = -zps[k].angle;
		f[k][0] = zps[k].amp * cos(phase);
		f[k][1] = zps[k].amp * sin(phase);
	}
	for(k = 1; k <= (N/2 - 1); k++){
		f[N - k][0] = f[k][0];
		f[N - k][1] = -f[k][1];
	}
	
	fnFFT_IFFT(f, coef);
}


void fir_filter_data(float *x, U16 n, float *firecoef, U16 firn, float *y){
	float x_buff[1024];
	U16 x_pointer, xp;
	U16 i, j;
	float rt;
	for(i = 0; i < n; i++){
        if(x_pointer >= firn){
          x_pointer = 0;
				}
        x_buff[x_pointer + 1] = x[i];
        x_pointer = x_pointer + 1;
        rt = 0;
        xp = x_pointer;
				for(j = 0; j < firn; j++){
          if(xp >= firn){
             xp = 0;
          }
          rt = rt + firecoef[j] * x_buff[xp];
          xp = xp + 1;
        }
        y[i] = rt;
		}
}